#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
// #include "sample_ive_queue.h"
// #include "sample_comm_svp.h"
#include "hi_ive.h"
#include "hi_comm_video.h"
#include "hi_comm_ive.h"
#include "hi_common.h"
#include "hi_type.h"
#include "mpi_vgs.h"
#include "mpi_sys.h"

#include "ive.h"
#include "vgs.h"
#include "queue.h"

#define SAMPLE_IVE_MAX(a, b) (((a) > (b)) ? (a) : (b))
#define SAMPLE_IVE_MIN(a, b) (((a) < (b)) ? (a) : (b))

#define SAMPLE_IVE_MUTEX_INIT_LOCK(mutex)       \
    do {                                        \
        (void)pthread_mutex_init(&(mutex), NULL); \
    } while (0)
#define SAMPLE_IVE_MUTEX_LOCK(mutex)      \
    do {                                  \
        (void)pthread_mutex_lock(&(mutex)); \
    } while (0)
#define SAMPLE_IVE_MUTEX_UNLOCK(mutex)      \
    do {                                    \
        (void)pthread_mutex_unlock(&(mutex)); \
    } while (0)
#define SAMPLE_IVE_MUTEX_DESTROY(mutex)      \
    do {                                     \
        (void)pthread_mutex_destroy(&(mutex)); \
    } while (0)
#define IVE_MMZ_FREE(phy, vir)                                         \
    do {                                                               \
        if (((phy) != 0) && ((vir) != 0)) {                            \
            HI_MPI_SYS_MmzFree((phy), (HI_VOID *)(HI_UINTPTR_T)(vir)); \
            (phy) = 0;                                                 \
            (vir) = 0;                                                 \
        }                                                              \
    } while (0)

#define SAMPLE_IVE_KCF_NODE_MAX_NUM 64
#define SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE 455680
#define SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE 832
// #define SAMPLE_IVE_KCF_COS_WINDOW_SIZE 64
#define SAMPLE_IVE_KCF_TEMP_BUF_SIZE 47616
// #define SAMPLE_IVE_KCF_HOG_FEATRUE_BUF_SIZE 47616
// #define SAMPLE_IVE_KCF_ROI_PADDING_MAX_WIDTH 1024
// #define SAMPLE_IVE_KCF_ROI_PADDING_MIN_WIDTH 40
// #define SAMPLE_IVE_KCF_ROI_PADDING_MAX_HEIGHT 1024
// #define SAMPLE_IVE_KCF_ROI_PADDING_MIN_HEIGHT 40
// #define SAMPLE_IVE_QUEUE_LEN 16
// #define SAMPLE_IVE_PROPOSAL_WIDTH 6 /* the number of proposal values */
// #define SAMPLE_IVE_COORDI_NUM 4     /* coordinate numbers */
#define SAMPLE_IVE_QUANT_BASE 4096  /* the base value */
// #define SAMPLE_IVE_SCORE_NUM 2      /* the num of RPN scores */
// #define SAMPLE_IVE_HALF 0.5f        /* the half value */
// #define SAMPLE_IVE_X_MIN_OFFSET     0
// #define SAMPLE_IVE_Y_MIN_OFFSET     1
// #define SAMPLE_IVE_X_MAX_OFFSET     2
// #define SAMPLE_IVE_Y_MAX_OFFSET     3
// #define SAMPLE_IVE_SCORE_OFFSET     4
// #define SAMPLE_IVE_SUPPRESS_FLAG_OFFSET   5
// #define SAMPLE_IVE_KCF_BASE_ANCHOR_ARR_NUM 4
// #define SAMPLE_IVE_KCF_IDX_0        0
// #define SAMPLE_IVE_KCF_IDX_1        1
// #define SAMPLE_IVE_KCF_IDX_2        2
// #define SAMPLE_IVE_KCF_IDX_3        3
// #define SAMPLE_IVE_KCF_IDX_4        4
// #define SAMPLE_IVE_KCF_SEG_1        1
// #define SAMPLE_IVE_KCF_SEG_2        2
// #define SAMPLE_IVE_KCF_CLASS_NUM    21
// #define SAMPLE_IVE_KCF_RATIO_ANCHOR_NUM  3
// #define SAMPLE_IVE_KCF_SCALE_ANCHOR_NUM  3
// #define SAMPLE_IVE_KCF_SCALE_0          (8 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_SCALE_1          (16 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_SCALE_2          (32 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_RATIO_0          (0.5 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_RATIO_1          (1 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_RATIO_2          (2 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_MIN_SIZE         16
// #define SAMPLE_IVE_KCF_FILTER_THR       0
#define SAMPLE_IVE_KCF_SPATIAL_SCALE    (0.0625 * SAMPLE_IVE_QUANT_BASE)
#define SAMPLE_IVE_KCF_NMS_THR          (0.7 * SAMPLE_IVE_QUANT_BASE)
// #define SAMPLE_IVE_KCF_NUM_BEFORE_NMS   6000
// #define SAMPLE_IVE_KCF_SCORE_THR       0.8f
// #define SAMPLE_IVE_KCF_VALID_NMS_THR   (0.3 * 4096)
#define SAMPLE_IVE_KCF_ROI_FIX_POINT    256
// #define SAMPLE_IVE_KCF_PAD_FIX_POINT    32
// #define SAMPLE_IVE_KCF_PAD_VALUE_2      2
// #define SAMPLE_IVE_KCF_PAD_VALUE_48     (1.5 * 32)
#define SAMPLE_IVE_KCF_CAR_IDX          7
// #define SAMPLE_IVE_KCF_IOU_NMS_THR      0.3f
// #define SAMPLE_IVE_KCF_INTER_FACTOR     (0.02 * 1024 * 32)
// #define SAMPLE_IVE_KCF_LAMDA            10
// #define SAMPLE_IVE_KCF_SIGMA            (0.5 * 256)
// #define SAMPLE_IVE_KCF_TRANC_ALFA       (0.2 * 4096)
// #define SAMPLE_IVE_KCF_RESP_THR         32
// #define SAMPLE_IVE_KCF_BLK_CNT_4        4
// #define SAMPLE_IVE_KCF_BLK_CNT_10       10
// #define SAMPLE_IVE_KCF_MAX_POOL_CNT     4
// #define SAMPLE_IVE_KCF_RPN_MAX_ROI_NUM  300
// #define SAMPLE_IVE_KCF_VPSS_MAX_CHN     2
// #define SAMPLE_IVE_KCF_MAX_PIC_SIZE     3
// #define SAMPLE_IVE_KCF_COLOR            0x0000FF00
#define SAMPLE_IVE_KCF_MAX_BBOX         64
// #define SAMPLE_IVE_KCF_MAX_FRM_NUM      2
// #define SAMPLE_IVE_KCF_CONV_OUT_NUM     2
// #define IVE_QUERY_SLEEP_TIME  100

static int s_s32CurQueueLen = 0;


typedef enum hiSAMPLE_IVE_CNN_GET_FRM_STATUS_E {
    CNN_GET_FRM_START = 0x0,
    CNN_GET_FRM_END = 0x1,

    CNN_GET_FRM_BUTT
} SAMPLE_IVE_CNN_GET_FRM_STATUS_E;

typedef enum hiSAMPLE_IVE_CNN_PROC_STATUS_E {
    CNN_PROC_START = 0x0,
    CNN_PROC_END = 0x1,

    CNN_PROC_BUTT
} SAMPLE_IVE_CNN_PROC_STATUS_E;

typedef struct hiSAMPLE_IVE_KCF_S {
    IVE_ROI_INFO_S astRoiInfo[SAMPLE_IVE_KCF_MAX_BBOX];
    HI_U32 u32RoiNum;
    IVE_MEM_INFO_S stTotalMem;
    IVE_MEM_INFO_S stListMem;
    IVE_MEM_INFO_S stCosWinX;
    IVE_MEM_INFO_S stCosWinY;
    IVE_MEM_INFO_S stGaussPeak;
    IVE_KCF_OBJ_LIST_S stObjList;
    IVE_KCF_PRO_CTRL_S stKcfProCtrl;
    IVE_KCF_BBOX_S astBbox[SAMPLE_IVE_KCF_MAX_BBOX];
    HI_U32 u32BboxObjNum;
    IVE_KCF_BBOX_CTRL_S stKcfBboxCtrl;
    HI_U3Q5 u3q5Padding;
    HI_U8 u8Reserved;

    SAMPLE_IVE_CNN_GET_FRM_STATUS_E enCnnGetFrmStat;
    SAMPLE_IVE_CNN_PROC_STATUS_E enCnnProcStat;
    // HI_BOOL bNewDetect;
    HI_BOOL bFirstDetect;

    SAMPLE_IVE_QUEUE_S *pstQueueHead;
    VIDEO_FRAME_INFO_S astFrameInfo[2];
    pthread_mutex_t CnnDetectMutex;
    pthread_mutex_t QueueMutex;
    pthread_mutex_t GetFrmMutex;
} SAMPLE_IVE_KCF_S;

static SAMPLE_IVE_KCF_S s_stIveKcfInfo = {0};

HI_S32 SAMPLE_COMM_IVE_CreateMemInfo(IVE_MEM_INFO_S *pstMemInfo, HI_U32 u32Size)
{
    HI_S32 s32Ret;

    if (pstMemInfo == NULL) 
    {
        printf("[FAIL] pstMemInfo is null\n");
        return HI_FAILURE;
    }
    pstMemInfo->u32Size = u32Size;
    s32Ret = HI_MPI_SYS_MmzAlloc(&pstMemInfo->u64PhyAddr, (HI_VOID **)&pstMemInfo->u64VirAddr, NULL, HI_NULL, u32Size);
    if (s32Ret != HI_SUCCESS) 
    {
        printf("[FAIL] Mmz Alloc fail,Error: %x\n", s32Ret);
        return HI_FAILURE;
    }

    return HI_SUCCESS;
}

int SAMPLE_IVE_KcfInit(SAMPLE_IVE_KCF_S *pstIveKcfInfo)
{
    HI_S32 s32Ret = HI_SUCCESS;
    HI_U32 u32Size;
    HI_U32 u32TotalSize;
    HI_S32 s32Len;

    memset_s(pstIveKcfInfo, sizeof(SAMPLE_IVE_KCF_S), 0, sizeof(SAMPLE_IVE_KCF_S));

    s32Ret = HI_MPI_IVE_KCF_GetMemSize(SAMPLE_IVE_KCF_NODE_MAX_NUM, &u32Size);
    /* (HOGFeatrue + Alpha + DstBuf) + Guasspeak + CosWinX + CosWinY + TmpBuf */
    if(s32Ret != HI_SUCCESS)
    {
        printf("[FAIL]  HI_MPI_IVE_KCF_GetMemSize failed: %x\n", s32Ret);
        goto FAIL_0;
    }
    
    u32TotalSize = u32Size + SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE + SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE +
        SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE + SAMPLE_IVE_KCF_TEMP_BUF_SIZE;
    s32Ret = SAMPLE_COMM_IVE_CreateMemInfo(&pstIveKcfInfo->stTotalMem, u32TotalSize);
    

    memset_s((HI_U8 *)(HI_UINTPTR_T)pstIveKcfInfo->stTotalMem.u64VirAddr, u32TotalSize, 0x0, u32TotalSize);

    pstIveKcfInfo->stListMem.u64PhyAddr = pstIveKcfInfo->stTotalMem.u64PhyAddr;
    pstIveKcfInfo->stListMem.u64VirAddr = pstIveKcfInfo->stTotalMem.u64VirAddr;
    pstIveKcfInfo->stListMem.u32Size = u32Size;

    pstIveKcfInfo->stGaussPeak.u64PhyAddr = pstIveKcfInfo->stListMem.u64PhyAddr + u32Size;
    pstIveKcfInfo->stGaussPeak.u64VirAddr = pstIveKcfInfo->stListMem.u64VirAddr + u32Size;
    pstIveKcfInfo->stGaussPeak.u32Size = SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;

    pstIveKcfInfo->stCosWinX.u64PhyAddr = pstIveKcfInfo->stGaussPeak.u64PhyAddr + SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;
    pstIveKcfInfo->stCosWinX.u64VirAddr = pstIveKcfInfo->stGaussPeak.u64VirAddr + SAMPLE_IVE_KCF_GAUSS_PEAK_TOTAL_SIZE;
    pstIveKcfInfo->stCosWinX.u32Size = SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;

    pstIveKcfInfo->stCosWinY.u64PhyAddr = pstIveKcfInfo->stCosWinX.u64PhyAddr + SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    pstIveKcfInfo->stCosWinY.u64VirAddr = pstIveKcfInfo->stCosWinX.u64VirAddr + SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    pstIveKcfInfo->stCosWinY.u32Size = SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;

    pstIveKcfInfo->stKcfProCtrl.stTmpBuf.u64PhyAddr =
        pstIveKcfInfo->stCosWinY.u64PhyAddr + SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    pstIveKcfInfo->stKcfProCtrl.stTmpBuf.u64VirAddr =
        pstIveKcfInfo->stCosWinY.u64VirAddr + SAMPLE_IVE_KCF_COS_WINDOW_TOTAL_SIZE;
    pstIveKcfInfo->stKcfProCtrl.stTmpBuf.u32Size = SAMPLE_IVE_KCF_TEMP_BUF_SIZE;

    pstIveKcfInfo->stKcfProCtrl.enCscMode = IVE_CSC_MODE_VIDEO_BT709_YUV2RGB;
    pstIveKcfInfo->stKcfProCtrl.u1q15InterFactor = 0.02 * 1024 * 32;
    pstIveKcfInfo->stKcfProCtrl.u0q16Lamda = 10;
    pstIveKcfInfo->stKcfProCtrl.u0q8Sigma = 0.5 * 256;
    pstIveKcfInfo->stKcfProCtrl.u4q12TrancAlfa = 0.2 * 4096;
    pstIveKcfInfo->stKcfProCtrl.u8RespThr = 32;

    pstIveKcfInfo->u3q5Padding = 1.5 * 32;

    pstIveKcfInfo->stKcfBboxCtrl.u32MaxBboxNum = SAMPLE_IVE_KCF_NODE_MAX_NUM;
    pstIveKcfInfo->stKcfBboxCtrl.s32RespThr = 0;

    s32Len = 16;
    pstIveKcfInfo->pstQueueHead = SAMPLE_IVE_QueueCreate(s32Len);
    if(pstIveKcfInfo->pstQueueHead == NULL)
    {
        printf("[FAIL]  SAMPLE_IVE_QueueCreate failed!\n");
        goto FAIL_0;
    }

    s32Ret = HI_MPI_IVE_KCF_CreateObjList(&pstIveKcfInfo->stListMem, SAMPLE_IVE_KCF_NODE_MAX_NUM, &pstIveKcfInfo->stObjList);
    if(s32Ret != HI_SUCCESS)
    {
        printf("[FAIL] HI_MPI_IVE_KCF_CreateObjList failed:%x\n", s32Ret);
        goto FAIL_0;
    }


    s32Ret = HI_MPI_IVE_KCF_CreateGaussPeak(pstIveKcfInfo->u3q5Padding, &pstIveKcfInfo->stGaussPeak);
    if(s32Ret != HI_SUCCESS)
    {
        printf("[FAIL]  HI_MPI_IVE_KCF_CreateGaussPeak failed: %x\n", s32Ret);
        goto FAIL_0;
    }


    s32Ret = HI_MPI_IVE_KCF_CreateCosWin(&pstIveKcfInfo->stCosWinX, &pstIveKcfInfo->stCosWinY);
    if(s32Ret != HI_SUCCESS)
    {
        printf("[FAIL]  HI_MPI_IVE_KCF_CreateCosWin failed: %x\n", s32Ret);
        goto FAIL_0;
    }

    pstIveKcfInfo->enCnnGetFrmStat = CNN_GET_FRM_END;
    pstIveKcfInfo->enCnnProcStat = CNN_PROC_END;
    pstIveKcfInfo->bFirstDetect = HI_FALSE;
    SAMPLE_IVE_MUTEX_INIT_LOCK(pstIveKcfInfo->CnnDetectMutex);
    SAMPLE_IVE_MUTEX_INIT_LOCK(pstIveKcfInfo->GetFrmMutex);
    SAMPLE_IVE_MUTEX_INIT_LOCK(pstIveKcfInfo->QueueMutex);

    return s32Ret;

FAIL_3:
    (HI_VOID)HI_MPI_IVE_KCF_DestroyObjList(&pstIveKcfInfo->stObjList);
FAIL_2:
    SAMPLE_IVE_QueueDestory(pstIveKcfInfo->pstQueueHead);
FAIL_1:
    IVE_MMZ_FREE(pstIveKcfInfo->stTotalMem.u64PhyAddr, pstIveKcfInfo->stTotalMem.u64VirAddr);
FAIL_0:
    return s32Ret;
}

int IVE_KCFInit()
{
    int s32Ret = 0;
    /* init Kcf param */
    printf("****************Im going to init Kcf !!!!!!!!!!!!!!!\n");
    s32Ret = SAMPLE_IVE_KcfInit(&s_stIveKcfInfo);
    return s32Ret;
}


static float SAMPLE_IVE_KcfCalcIOU(IVE_RECT_S24Q8_S *pstRect1, IVE_RECT_S24Q8_S *pstRect2)
{
    HI_S32 s32MinX, s32MinY, s32MaxX, s32MaxY;
    HI_FLOAT fArea1, fArea2, fInterArea, fIou;
    HI_S32 s32Width, s32Height;

    s32MinX = SAMPLE_IVE_MAX(pstRect1->s24q8X / SAMPLE_IVE_KCF_ROI_FIX_POINT,
        pstRect2->s24q8X / SAMPLE_IVE_KCF_ROI_FIX_POINT);
    s32MinY = SAMPLE_IVE_MAX(pstRect1->s24q8Y / SAMPLE_IVE_KCF_ROI_FIX_POINT,
        pstRect2->s24q8Y / SAMPLE_IVE_KCF_ROI_FIX_POINT);
    s32MaxX = SAMPLE_IVE_MIN(pstRect1->s24q8X / SAMPLE_IVE_KCF_ROI_FIX_POINT + pstRect1->u32Width,
        pstRect2->s24q8X / SAMPLE_IVE_KCF_ROI_FIX_POINT + pstRect2->u32Width);
    s32MaxY = SAMPLE_IVE_MIN(pstRect1->s24q8Y / SAMPLE_IVE_KCF_ROI_FIX_POINT + pstRect1->u32Height,
        pstRect2->s24q8Y / SAMPLE_IVE_KCF_ROI_FIX_POINT + pstRect2->u32Height);

    s32Width = s32MaxX - s32MinX + 1;
    s32Height = s32MaxY - s32MinY + 1;

    s32Width = s32Width > 0 ? s32Width : 0;
    s32Height = s32Height > 0 ? s32Height : 0;

    fInterArea = (HI_FLOAT)((HI_U32)s32Width * (HI_U32)s32Height);

    fArea1 = (HI_FLOAT)(pstRect1->u32Width * pstRect1->u32Height);
    fArea2 = (pstRect2->u32Width * pstRect2->u32Height);

    fIou = fInterArea / (fArea1 + fArea2 - fInterArea);

    return fIou;
}


// static int SAMPLE_IVE_KCF_ObjIOU(IVE_ROI_INFO_S astRoiInfo[], HI_U32 u32RoiNum, IVE_KCF_BBOX_S astBbox[],
//     HI_U32 u32BboxNum, IVE_ROI_INFO_S astNewRoiInfo[], HI_U32 u32MaxRoiNum, HI_U32 *pu32NewRoiNum)
// {
//     HI_S32 s32Ret = HI_SUCCESS;
//     HI_FLOAT f32NmsThr = 0.3f;
//     HI_BOOL bTrackOk;
//     HI_U32 u32NewRoiNum = 0;
//     HI_U32 i, j;
//     if (u32BboxNum == 0) 
//     {
//         s32Ret = memcpy_s(astNewRoiInfo, sizeof(IVE_ROI_INFO_S) * u32RoiNum, astRoiInfo, sizeof(IVE_ROI_INFO_S) * u32RoiNum);
//         if (s32Ret != HI_SUCCESS) 
//         {
//             printf("[FAIL] memcpy_s roi_info failed!\n");
//             return HI_ERR_IVE_ILLEGAL_PARAM;
//         }
//         *pu32NewRoiNum = u32RoiNum;
//         return s32Ret;
//     }
//     if (u32RoiNum == 0) 
//     {
//         *pu32NewRoiNum = 0;
//         return s32Ret;
//     }
//     // for (i = 0; i < u32RoiNum; i++) 
//     // {
//         // for (j = 0; j < u32BboxNum; j++) 
//         // {
//             HI_BOOL bSuppressed;
//             HI_FLOAT f32Iou;
//             if (astRoiInfo[0].u32RoiId == 0) 
//             {
//                 break;
//             }
//             if ((astBbox[0].stRoiInfo.u32RoiId == 0) || (HI_FALSE == astBbox[0].bTrackOk)) 
//             {
//                 continue;
//             }
//             f32Iou = SAMPLE_IVE_KcfCalcIOU(&astRoiInfo[0].stRoi, &astBbox[0].stRoiInfo.stRoi);
//             bSuppressed = (HI_BOOL)(f32Iou >= f32NmsThr);
//             if (bSuppressed) 
//             {
//                 //判断目标区域跟踪状态，三个参数为目标区域信息指针、目标区域跟踪结果信息指针、目标区域跟踪状态标志指针
//                 s32Ret = HI_MPI_IVE_KCF_JudgeObjBboxTrackState(&astRoiInfo[0], &astBbox[0], &bTrackOk);
//                 if (s32Ret != HI_SUCCESS) 
//                 {
//                     printf("[FAIL] HI_MPI_IVE_KCF_JudgeObjBboxTrackState failed: %x\n", s32Ret);
//                     return s32Ret;
//                 }
//                 // do not update the bbox's u32RoiId
//                 if (bTrackOk) 
//                 {
//                     astBbox[0].stRoiInfo.stRoi.s24q8X = astRoiInfo[0].stRoi.s24q8X;
//                     astBbox[0].stRoiInfo.stRoi.s24q8Y = astRoiInfo[0].stRoi.s24q8Y;
//                     astBbox[0].stRoiInfo.stRoi.u32Width = astRoiInfo[0].stRoi.u32Width;
//                     astBbox[0].stRoiInfo.stRoi.u32Height = astRoiInfo[0].stRoi.u32Height;
//                     astBbox[0].bRoiRefresh = HI_TRUE;
//                     astRoiInfo[0].u32RoiId = 0;
//                 }
//                 else 
//                 {
//                     astBbox[0].bTrackOk = HI_FALSE;
//                 }
//                 break;
//             }
//         // }
//         if (u32NewRoiNum == u32MaxRoiNum) 
//         {
//             printf("[FAIL] reaching the maxRoiNum(%d), the rest roi will be cast away!\n", u32MaxRoiNum);
//             *pu32NewRoiNum = u32NewRoiNum;
//             return HI_SUCCESS;
//         }
//         // u32RoiId!=0, means it hasn't been suppressed
//         if (astRoiInfo[0].u32RoiId != 0) 
//         {
//             (HI_VOID)memcpy_s(&astNewRoiInfo[u32NewRoiNum++], sizeof(IVE_ROI_INFO_S), &astRoiInfo[0],
//                 sizeof(IVE_ROI_INFO_S));
//         }
//     // }
//     *pu32NewRoiNum = u32NewRoiNum;
//     return s32Ret;
// }


static void SAMPLE_IVE_RectToPoint(IVE_KCF_BBOX_S astBbox[], HI_U32 u32BboxNum, HI_U32 u32BboxObjNum,
    SAMPLE_RECT_ARRAY_S *pstRect)
{
    HI_U32 i;
    HI_U32 tmp_ux, tmp_uy;
    HI_S32 tmp_sx, tmp_sy;

    pstRect->u16Num = 0;
    // for (i = 0; i < u32BboxObjNum && i < u32BboxNum; i++) 
    // {
        tmp_sx = astBbox[0].stRoiInfo.stRoi.s24q8X / SAMPLE_IVE_KCF_ROI_FIX_POINT;
        tmp_sy = astBbox[0].stRoiInfo.stRoi.s24q8Y / SAMPLE_IVE_KCF_ROI_FIX_POINT;

        if (tmp_sx < 0) 
        {
            tmp_ux = *((hi_u32 *)&tmp_sx);
            tmp_ux = tmp_ux & (~1);
            tmp_sx = *((hi_s32 *)&tmp_ux);
        }
        if (tmp_sy < 0) 
        {
            tmp_uy = *((hi_u32 *)&tmp_sy);
            tmp_uy = tmp_uy & (~1);
            tmp_sy = *((hi_s32 *)&tmp_sy);
        }
        if (tmp_sx > 0) 
        {
            tmp_ux = (hi_u32)tmp_sx;
            tmp_ux = tmp_ux & (~1);
            tmp_sx = tmp_ux;
        }
        if (tmp_sy > 0) 
        {
            tmp_uy = (hi_u32)tmp_sy;
            tmp_uy = tmp_uy & (~1);
            tmp_sy = tmp_uy;
        }

        pstRect->astRect[0].astPoint[0].s32X = tmp_sx;
        pstRect->astRect[0].astPoint[0].s32Y = tmp_sy;
        if ((pstRect->astRect[0].astPoint[0].s32X < 0) || (pstRect->astRect[0].astPoint[0].s32Y < 0)) 
        {
            pstRect->astRect[0].astPoint[0].s32X = 0;
            pstRect->astRect[0].astPoint[0].s32Y = 0;
            printf("***************x and y < 0!!!!!!!!!!\n");
        }
        pstRect->astRect[0].astPoint[1].s32X = pstRect->astRect[0].astPoint[0].s32X + (astBbox[0].stRoiInfo.stRoi.u32Width & (~1));
        pstRect->astRect[0].astPoint[1].s32Y = pstRect->astRect[0].astPoint[0].s32Y;
        
        pstRect->astRect[0].astPoint[2].s32X = pstRect->astRect[0].astPoint[0].s32X + (astBbox[0].stRoiInfo.stRoi.u32Width & (~1));
        pstRect->astRect[0].astPoint[2].s32Y = pstRect->astRect[0].astPoint[0].s32Y + (astBbox[0].stRoiInfo.stRoi.u32Height & (~1));
        
        pstRect->astRect[0].astPoint[3].s32X = pstRect->astRect[0].astPoint[0].s32X;
        pstRect->astRect[0].astPoint[3].s32Y = pstRect->astRect[0].astPoint[0].s32Y + (astBbox[0].stRoiInfo.stRoi.u32Height & (~1));

        pstRect->u16Num++;
    // }
}


static int SAMPLE_IVE_DispProcess(VIDEO_FRAME_INFO_S *pstFrameInfo, SAMPLE_RECT_ARRAY_S *pstRect)
{
    int s32Ret;
    VO_LAYER voLayer = 0;
    VO_CHN voChn = 0;
    int s32MilliSec = 20000;

    s32Ret = SAMPLE_COMM_VGS_FillRect(pstFrameInfo, pstRect, 0x0000FF00);
    if (s32Ret != HI_SUCCESS) 
    {
        printf("[FAIL] SAMPLE_COMM_VGS_FillRect failed: %x\n", s32Ret);
    }

    // s32Ret = HI_MPI_VO_SendFrame(voLayer, voChn, pstFrameInfo, s32MilliSec);
    // if (s32Ret != HI_SUCCESS) 
    // {        
    //     printf("[FAIL] HI_MPI_VO_SendFrame failed!\n");
    //     return s32Ret;
    // }

    return s32Ret;
}


/* function : Kcf tracking and vo display thread entry */
int SAMPLE_IVE_KcfTracking(VIDEO_FRAME_INFO_S *pstFrmInfo, IVE_ROI_INFO_S *RoiInfo, HI_BOOL re_track,int *buffer)
{
    static IVE_KCF_BBOX_S astBbox[SAMPLE_IVE_KCF_MAX_BBOX] = {0};
    static IVE_ROI_INFO_S astRoiInfo[SAMPLE_IVE_KCF_MAX_BBOX] = {0};
    // IVE_ROI_INFO_S astNewRoiInfo[SAMPLE_IVE_KCF_MAX_BBOX] = {0};
    static SAMPLE_RECT_ARRAY_S stRect = {0};
    static int cnt = 0;
    static int buff[4] = {0};
    HI_U32 u32BboxNum = 0;
    HI_U32 u32RoiNum = 1;
    HI_U32 u32MaxNewRoiNum = SAMPLE_IVE_KCF_MAX_BBOX;
    HI_U32 u32NewRoiNum = 1;
    IVE_HANDLE hIveHandle;
    HI_BOOL bInstant = HI_TRUE;
    IVE_IMAGE_S stSrc;
    SAMPLE_IVE_NODE_S *pstQueueNode = NULL;
    HI_S32 s32QueueLen;
    HI_S32 s32Ret;
    HI_BOOL bFinish = HI_FALSE;
    HI_BOOL bBlock = HI_TRUE;

    // SAMPLE_IVE_MUTEX_LOCK(s_stIveKcfInfo.QueueMutex);
    
    // if (s_stIveKcfInfo.pstQueueHead == NULL)
    // {
    //     return 0;
    // }
    // s32QueueLen = s_s32CurQueueLen;
    
    // if (s32QueueLen <= 0) 
    // {
    //     SAMPLE_IVE_MUTEX_UNLOCK(s_stIveKcfInfo.QueueMutex);
    // }
    
    if(re_track == HI_SUCCESS || cnt == 0)
    {
        astRoiInfo[0] = *RoiInfo;
        cnt ++;
    }


    //把帧的信息放到stSrc中
    stSrc.au64PhyAddr[0] = pstFrmInfo->stVFrame.u64PhyAddr[0];
    stSrc.au64PhyAddr[1] = pstFrmInfo->stVFrame.u64PhyAddr[1];
    stSrc.au32Stride[0] = pstFrmInfo->stVFrame.u32Stride[0];
    stSrc.au32Stride[1] = pstFrmInfo->stVFrame.u32Stride[1];
    stSrc.u32Width = pstFrmInfo->stVFrame.u32Width;
    stSrc.u32Height = pstFrmInfo->stVFrame.u32Height;
    if (pstFrmInfo->stVFrame.enPixelFormat == PIXEL_FORMAT_YVU_SEMIPLANAR_420) 
    {
        stSrc.enType = IVE_IMAGE_TYPE_YUV420SP;
    }

    
    // if (s_stIveKcfInfo.bNewDetect) 
    // {
        /*
        //看看有没有新的ROI
        SAMPLE_IVE_MUTEX_LOCK(s_stIveKcfInfo.CnnDetectMutex);
        // s_stIveKcfInfo.bNewDetect = HI_FALSE;
        u32RoiNum = s_stIveKcfInfo.u32RoiNum;
        s_stIveKcfInfo.u32RoiNum = 0;
        memcpy_s(astRoiInfo, sizeof(IVE_ROI_INFO_S) * u32RoiNum, s_stIveKcfInfo.astRoiInfo,
            sizeof(IVE_ROI_INFO_S) * u32RoiNum);
        SAMPLE_IVE_MUTEX_UNLOCK(s_stIveKcfInfo.CnnDetectMutex);

        //计算新的Roi和原来Roi的Iou，判断是否是一个Roi？
        //第一次进入时，会把astRoiInfo复制给astNewRoiInfo，把u32RoiNum复制给u32NewRoiNum然后返回0
        s32Ret = SAMPLE_IVE_KCF_ObjIOU(astRoiInfo, u32RoiNum, astBbox, u32BboxNum, 
            astNewRoiInfo, u32MaxNewRoiNum, &u32NewRoiNum);
        if (s32Ret != HI_SUCCESS) 
        {
            printf("[FAIL] SAMPLE_IVE_KCF_ObjIOU failed!\n");
            goto FAIL;
        }

        //更新目标信息，三个参数分别为目标链表指针、目标区域跟踪结果信息、目标区域跟踪结果数目。
        s32Ret = HI_MPI_IVE_KCF_ObjUpdate(&s_stIveKcfInfo.stObjList, astBbox, u32BboxNum);
        if (s32Ret != HI_SUCCESS) 
        {
            printf("[FAIL] HI_MPI_IVE_KCF_ObjUpdate failed: %x\n", s32Ret);
            goto FAIL;
        }
        */
        // memcpy_s(astRoiInfo, sizeof(IVE_ROI_INFO_S) * u32RoiNum, s_stIveKcfInfo.astRoiInfo,
        //     sizeof(IVE_ROI_INFO_S) * u32RoiNum);
        
        //更新目标信息，三个参数分别为目标链表指针、目标区域跟踪结果信息、目标区域跟踪结果数目。
        s32Ret = HI_MPI_IVE_KCF_ObjUpdate(&s_stIveKcfInfo.stObjList, astBbox, u32BboxNum);
        if (s32Ret != HI_SUCCESS) 
        {
            printf("[FAIL] HI_MPI_IVE_KCF_ObjUpdate failed: %x\n", s32Ret);
            goto FAIL;
        }
        
        u32BboxNum = 1;

        //获取需要训练的目标对象，7个参数分别为目标区域放大倍数、目标区域信息、
        //目标区域数目、水平方向汉宁窗指针、垂直方向汉宁窗指针、高斯峰值表指针、目标对象链表指针(这个是输出)
        s32Ret = HI_MPI_IVE_KCF_GetTrainObj(s_stIveKcfInfo.u3q5Padding, astRoiInfo, u32NewRoiNum,
            &s_stIveKcfInfo.stCosWinX, &s_stIveKcfInfo.stCosWinY, &s_stIveKcfInfo.stGaussPeak,
            &s_stIveKcfInfo.stObjList);
        if (s32Ret != HI_SUCCESS) 
        {
            printf("[FAIL] HI_MPI_IVE_KCF_GetTrainObj failed: %x\n", s32Ret);
            // goto FAIL;
        }
    // }

    //进行KCF跟踪
    // if (s_stIveKcfInfo.stObjList.u32TrackObjNum != 0 || s_stIveKcfInfo.stObjList.u32TrainObjNum != 0) 
    // {
        
        //将目标交给硬件进行KCF处理,5个参数分别为handle 指针、源数据指针、目标链表指针、控制指针、及时返回结果标志
        s32Ret = HI_MPI_IVE_KCF_Process(&hIveHandle, &stSrc, &s_stIveKcfInfo.stObjList,&s_stIveKcfInfo.stKcfProCtrl, bInstant);   
        if (s32Ret != HI_SUCCESS)
        {
            printf("[FAIL] HI_MPI_IVE_KCF_Process failed: %x\n", s32Ret);
            goto FAIL;
        }

        //查询已创建任务完成情况,三个参数分别为任务的handle、任务完成状态指针、是否阻塞查询标志
        s32Ret = HI_MPI_IVE_Query(hIveHandle, &bFinish, bBlock);
        while (s32Ret == HI_ERR_IVE_QUERY_TIMEOUT) 
        {
            usleep(100);
            s32Ret = HI_MPI_IVE_Query(hIveHandle, &bFinish, bBlock);
        }
        if(s32Ret != HI_SUCCESS)
        {
            printf("[FAIL] HI_MPI_IVE_Query failed: %x\n", s32Ret);
            return s32Ret;
        }

        //获取目标区域跟踪结果信息，四个参数分别为目标链表指针、目标区域跟踪结果信息、目标区域跟踪结果数目指针、控制指针。
        s32Ret = HI_MPI_IVE_KCF_GetObjBbox(&s_stIveKcfInfo.stObjList, astBbox, &u32BboxNum,&s_stIveKcfInfo.stKcfBboxCtrl);
        if (s32Ret != HI_SUCCESS)
        {
            printf("[FAIL] HI_MPI_IVE_KCF_GetObjBbox failed: %x\n", s32Ret);
            // return s32Ret;
        }
        //把每个roi的四个点坐标放在stRect中
        SAMPLE_IVE_RectToPoint(astBbox, SAMPLE_IVE_KCF_MAX_BBOX, u32BboxNum, &stRect);
    // } 
    // else 
    // {
    //     stRect.u16Num = 0;
    // }

    s32Ret = SAMPLE_IVE_DispProcess(pstFrmInfo, &stRect);
    if (s32Ret != HI_SUCCESS)
    {
        printf("[FAIL] SAMPLE_IVE_DispProcess failed: %x\n", s32Ret);
        return s32Ret;
    }

    buff[0] = stRect.astRect[0].astPoint[0].s32X;
    buff[1] = stRect.astRect[0].astPoint[0].s32Y;
    buff[2] = stRect.astRect[0].astPoint[1].s32X-stRect.astRect[0].astPoint[0].s32X;
    buffer[0] = buff[0];
    buffer[1] = buff[1];
    buffer[2] = buff[2];
    return 0;

    FAIL:
        // HI_MPI_VPSS_ReleaseChnFrame(0, 1, &pstQueueNode->stFrameInfo);
        // SAMPLE_IVE_QueueFreeNode(pstQueueNode);
        return -1;
    
   
}

HI_VOID SAMPLE_IVE_KcfDeInit(SAMPLE_IVE_KCF_S *pstIveKcfInfo)
{
    SAMPLE_IVE_NODE_S *pstQueueNode = NULL;

    // if (CNN_PROC_START == pstIveKcfInfo->enCnnProcStat) {
    //     (HI_VOID)HI_MPI_VPSS_ReleaseChnFrame(0, 2, &pstIveKcfInfo->astFrameInfo[1]);
    //     SAMPLE_IVE_MUTEX_LOCK(pstIveKcfInfo->GetFrmMutex);
    //     pstIveKcfInfo->enCnnGetFrmStat = CNN_GET_FRM_END;
    //     pstIveKcfInfo->enCnnProcStat = CNN_PROC_END;
    //     SAMPLE_IVE_MUTEX_UNLOCK(pstIveKcfInfo->GetFrmMutex);
    // }

    SAMPLE_IVE_MUTEX_DESTROY(pstIveKcfInfo->QueueMutex);
    SAMPLE_IVE_MUTEX_DESTROY(pstIveKcfInfo->GetFrmMutex);
    SAMPLE_IVE_MUTEX_DESTROY(pstIveKcfInfo->CnnDetectMutex);
    (HI_VOID)HI_MPI_IVE_KCF_DestroyObjList(&pstIveKcfInfo->stObjList);

    while (!SAMPLE_IVE_QueueIsEmpty(pstIveKcfInfo->pstQueueHead)) 
    {
        pstQueueNode = SAMPLE_IVE_QueueGetNode(pstIveKcfInfo->pstQueueHead);
        if (pstQueueNode != NULL) 
        {
            (HI_VOID)HI_MPI_VPSS_ReleaseChnFrame(0, 1, &pstQueueNode->stFrameInfo);
            SAMPLE_IVE_QueueFreeNode(pstQueueNode);
        }
    }
    SAMPLE_IVE_QueueDestory(pstIveKcfInfo->pstQueueHead);
    IVE_MMZ_FREE(pstIveKcfInfo->stTotalMem.u64PhyAddr, pstIveKcfInfo->stTotalMem.u64VirAddr);

    return;
}

void IVE_KCFDeInit()
{
    SAMPLE_IVE_KcfDeInit(&s_stIveKcfInfo);
}